package com.dkts.oil.router.impl;

import com.dkts.oil.converter.GeometryConverter;
import org.locationtech.jts.geom.*;

import java.util.*;

public class ObstacleAvoidance {

    // 定义A*算法中用于寻找路径的节点
    public static class Node {
        Coordinate coordinate;
        double gCost; // 从起点到当前节点的实际代价
        double hCost; // 从当前节点到目标节点的启发式代价
        double fCost; // 总代价 f = g + h
        Node parent;

        public Node(Coordinate coordinate, double gCost, double hCost) {
            this.coordinate = coordinate;
            this.gCost = gCost;
            this.hCost = hCost;
            this.fCost = gCost + hCost;
        }
    }

    // A*算法实现
    public static List<Coordinate> calculateBypassPath(LineString lineString, Geometry obstacle) {
        List<Coordinate> bypassPath = new ArrayList<>();

        // 起点和终点
        Coordinate start = lineString.getCoordinates()[0];
        Coordinate goal = lineString.getCoordinates()[lineString.getCoordinates().length - 1];

        // 定义开放列表和关闭列表
        PriorityQueue<Node> openList = new PriorityQueue<>(Comparator.comparingDouble(n -> n.fCost));
        Set<Coordinate> closedList = new HashSet<>();

        // 初始节点
        Node startNode = new Node(start, 0, calculateHeuristic(start, goal));
        openList.add(startNode);

        // A*算法主循环
        while (!openList.isEmpty()) {
            Node currentNode = openList.poll();

            // 如果目标节点被找到，回溯路径
            if (currentNode.coordinate.equals(goal)) {
                Node temp = currentNode;
                while (temp != null) {
                    bypassPath.add(temp.coordinate);
                    temp = temp.parent;
                }
                Collections.reverse(bypassPath);
                break;
            }

            closedList.add(currentNode.coordinate);

            // 计算相邻节点
            for (Coordinate neighbor : getNeighbors(currentNode.coordinate)) {
                // 如果该邻居是障碍物或已经在关闭列表中，则跳过
                if (isObstacle(neighbor, obstacle) || closedList.contains(neighbor)) {
                    continue;
                }

                // 计算G值和H值
                double gCost = currentNode.gCost + start.distance(neighbor); // 假设均为直线代价
                double hCost = calculateHeuristic(neighbor, goal);

                // 如果邻居在开放列表中且新的路径代价更高，则跳过
                boolean isNewPath = true;
                for (Node openNode : openList) {
                    if (openNode.coordinate.equals(neighbor) && gCost >= openNode.gCost) {
                        isNewPath = false;
                        break;
                    }
                }

                if (isNewPath) {
                    Node neighborNode = new Node(neighbor, gCost, hCost);
                    neighborNode.parent = currentNode;
                    openList.add(neighborNode);
                }
            }
        }

        return bypassPath;
    }

    // 计算启发式代价（H值），这里使用欧几里得距离
    private static double calculateHeuristic(Coordinate current, Coordinate goal) {
        return current.distance(goal);
    }

    // 获取相邻节点
    private static List<Coordinate> getNeighbors(Coordinate current) {
        List<Coordinate> neighbors = new ArrayList<>();
        double stepSize = 0.0001; // 节点间隔

        // 上下左右四个方向
        neighbors.add(new Coordinate(current.x + stepSize, current.y));
        neighbors.add(new Coordinate(current.x - stepSize, current.y));
        neighbors.add(new Coordinate(current.x, current.y + stepSize));
        neighbors.add(new Coordinate(current.x, current.y - stepSize));

        return neighbors;
    }

    // 判断一个坐标是否为障碍物
    private static boolean isObstacle(Coordinate point, Geometry obstacle) {
        return obstacle.contains(GeometryConverter.locationtechGeometryFactory.createPoint( point));
    }
}
